function tau(tau_out, parms, q, dq, ddq)
#
    x0 = sin(q[2])
    x1 = -dq[2]
    x2 = cos(q[2])
    x3 = -x2
    x4 = dq[1]*x3
    x5 = ddq[1]*x0 + x1*x4
    x6 = ddq[1]*x3
    x7 = dq[1]*x0
    x8 = -x1
    x9 = x6 + x7*x8
    x10 = -ddq[2]
    x11 = -0.41*x0^2 - 0.41*x2^2
    x12 = -0.41*dq[2]
    x13 = x11*x6 + x12*x7
    x14 = x11*x4
    x15 = -parms[17]
    x16 = parms[18]*x7 + parms[20]*x14 + x15*x4
    x17 = parms[13]*x7 + parms[15]*x4 + parms[16]*x1 + parms[17]*x12
    x18 = cos(q[3])
    x19 = x18*x5
    x20 = sin(q[3])
    x21 = x20*x9
    x22 = x20*x7
    x23 = x18*x4
    x24 = x22 - x23
    x25 = -dq[3]
    x26 = x19 + x21 + x24*x25
    x27 = ddq[3] + x10
    x28 = x20*x5
    x29 = -x18
    x30 = x18*x7
    x31 = x20*x4
    x32 = x30 + x31
    x33 = dq[3]*x32 + x28 + x29*x9
    x34 = 9.81*x0
    x35 = -0.41*ddq[2] - 9.81*x2
    x36 = x12*x20 + 0.0098*x22 - 0.0098*x23
    x37 = dq[3]*x36 - 0.0098*x19 + x20*x34 - 0.0098*x21 + x29*x35
    x38 = dq[3] + x1
    x39 = -parms[28]
    x40 = parms[23]*x32 + parms[25]*x38 + parms[26]*x24 + parms[27]*x14 + x36*x39
    x41 = cos(q[4])
    x42 = x26*x41
    x43 = sin(q[4])
    x44 = x27*x43
    x45 = dq[4]/2
    x46 = sqrt(3)
    x47 = x24/2
    x48 = x38*x41
    x49 = x32*x43
    x50 = x46*x47 + x48/2 - x49/2
    x51 = x46/2
    x52 = dq[4]*x51
    x53 = -x52
    x54 = -x46/2
    x55 = x47 + x48*x54 + x49*x51
    x56 = x42 + x44 + x45*x50 + x53*x55
    x57 = x27*x41
    x58 = x26*x43
    x59 = -x45
    x60 = x32*x41
    x61 = x38*x43
    x62 = x60 + x61
    x63 = ddq[4]*x51 + x33*x51 + x57/2 - x58/2 + x59*x62
    x64 = ddq[4]/2 + x33/2 + x51*x58 + x52*x62 + x54*x57
    x65 = -0.0098*x18
    x66 = x12*x29 - 0.0098*x30 - 0.0098*x31
    x67 = x18*x34 + x20*x35 + x25*x66 + 0.0098*x28 + x65*x9
    x68 = x43*x67
    x69 = x13*x41
    x70 = -0.125040827473476*x46
    x71 = x14*x43 + x36*x41 - 0.250081654946951*x48 + 0.250081654946951*x49
    x72 = x37/2 + x42*x70 + x44*x70 + x51*x68 + x52*x71 + x54*x69
    x73 = x50 + x52
    x74 = x45 + x55
    x75 = x14*x41
    x76 = x36*x43
    x77 = x51*x66 + 0.125040827473476*x60 + 0.125040827473476*x61 + x75/2 - x76/2
    x78 = -parms[38]
    x79 = parms[33]*x62 + parms[35]*x73 + parms[36]*x74 + parms[37]*x77 + x71*x78
    x80 = cos(q[5])
    x81 = x56*x80
    x82 = sin(q[5])
    x83 = x63*x82
    x84 = x73*x80
    x85 = x62*x82
    x86 = x51*x74 + x84/2 - x85/2
    x87 = dq[5]/2
    x88 = x51*x85 + x54*x84 + x74/2
    x89 = dq[5]*x51
    x90 = -x89
    x91 = x81 + x83 + x86*x87 + x88*x90
    x92 = x63*x80
    x93 = x56*x82
    x94 = x73*x82
    x95 = x62*x80
    x96 = x94 + x95
    x97 = -x87
    x98 = ddq[5]*x51 + x51*x64 + x92/2 - x93/2 + x96*x97
    x99 = ddq[5]/2 + x51*x93 + x54*x92 + x64/2 + x89*x96
    x100 = x51*x76 + x54*x75 + x60*x70 + x61*x70 + x66/2
    x101 = x100*x53 + x13*x43 + x41*x67 + x45*x77 - 0.250081654946951*x57 + 0.250081654946951*x58
    x102 = x101*x82
    x103 = x37*x51 + 0.125040827473476*x42 + 0.125040827473476*x44 + x59*x71 - x68/2 + x69/2
    x104 = x103*x80
    x105 = -0.0427816549469513*x46
    x106 = x71*x80 + x77*x82 - 0.0855633098939025*x84 + 0.0855633098939025*x85
    x107 = x102*x51 + x104*x54 + x105*x81 + x105*x83 + x106*x89 + x72/2
    x108 = x77*x80
    x109 = x71*x82
    x110 = x100*x51 + x108/2 - x109/2 + 0.0427816549469513*x94 + 0.0427816549469513*x95
    x111 = x100/2 + x105*x94 + x105*x95 + x108*x54 + x109*x51
    x112 = x86 + x89
    x113 = -x112
    x114 = parms[47]*x113 + parms[48]*x96 + parms[50]*x111
    x115 = x87 + x88
    x116 = -x106
    x117 = parms[43]*x96 + parms[45]*x112 + parms[46]*x115 + parms[47]*x110 + parms[48]*x116
    x118 = cos(q[6])
    x119 = x118*x91
    x120 = sin(q[6])
    x121 = x120*x98
    x122 = x120*x96
    x123 = x112*x118
    x124 = x122 - x123
    x125 = -dq[6]
    x126 = x119 + x121 + x124*x125
    x127 = x120*x91
    x128 = -x118
    x129 = x118*x96
    x130 = x112*x120
    x131 = x129 + x130
    x132 = -x125
    x133 = x127 + x128*x98 + x131*x132
    x134 = -x99
    x135 = -ddq[6] + x134
    x136 = -x107
    x137 = -x115
    x138 = x125 + x137
    x139 = x106*x120 + x110*x128 - 0.202781654946951*x129 - 0.202781654946951*x130
    x140 = x106*x118 + x110*x120 + 0.202781654946951*x122 - 0.202781654946951*x123
    x141 = -parms[58]
    x142 = parms[53]*x131 + parms[55]*x124 + parms[56]*x138 + parms[57]*x139 + x140*x141
    x143 = -x111
    x144 = -parms[57]
    x145 = parms[58]*x131 + parms[60]*x143 + x124*x144
    x146 = x101*x80 + x103*x82 + x110*x87 + x111*x90 - 0.0855633098939025*x92 + 0.0855633098939025*x93
    x147 = -x102/2 + x104/2 + x106*x97 + x51*x72 + 0.0427816549469513*x81 + 0.0427816549469513*x83
    x148 = -0.202781654946951*x119 + x120*x146 - 0.202781654946951*x121 + x128*x147 + x132*x140
    x149 = -parms[59]
    x150 = parms[57]*x138 + parms[60]*x139 + x131*x149
    x151 = -x150
    x152 = parms[52]*x131 + parms[54]*x124 + parms[55]*x138 + parms[59]*x140 + x143*x144
    x153 = parms[51]*x126 + parms[52]*x133 + parms[53]*x135 + parms[58]*x136 + x124*x142 - x138*x152 + x139*x145 + x143*x151 + x148*x149
    x154 = -0.202781654946951*x118
    x155 = x118*x146 + x120*x147 + x125*x139 + 0.202781654946951*x127 + x154*x98
    x156 = parms[59]*x124 + parms[60]*x140 + x138*x141
    x157 = parms[51]*x131 + parms[52]*x124 + parms[53]*x138 + parms[58]*x143 + x139*x149
    x158 = -x131
    x159 = parms[52]*x126 + parms[54]*x133 + parms[55]*x135 + parms[59]*x155 + x136*x144 + x138*x157 - x140*x145 + x142*x158 + x143*x156
    x160 = -parms[49]
    x161 = parms[47]*x115 + parms[50]*x110 + x160*x96
    x162 = parms[42]*x96 + parms[44]*x112 + parms[45]*x115 + parms[47]*x143 + parms[49]*x106
    x163 = parms[59]*x133 + parms[60]*x155 + x124*x145 + x135*x141 + x138*x151
    x164 = x120*x163
    x165 = parms[57]*x135 + parms[60]*x148 + x126*x149 + x138*x156 + x145*x158
    x166 = parms[41]*x91 + parms[42]*x98 + parms[43]*x99 + parms[48]*x107 + x110*x114 + x112*x117 + x118*x153 + x120*x159 + x137*x162 + x143*x161 + x147*x160 + x154*x165 + 0.202781654946951*x164
    x167 = -parms[37]
    x168 = parms[38]*x62 + parms[40]*x100 + x167*x73
    x169 = -parms[39]
    x170 = parms[37]*x74 + parms[40]*x77 + x169*x62
    x171 = -x170
    x172 = parms[32]*x62 + parms[34]*x73 + parms[35]*x74 + parms[39]*x71 + x100*x167
    x173 = parms[48]*x137 + parms[49]*x112 + parms[50]*x106
    x174 = parms[41]*x96 + parms[42]*x112 + parms[43]*x115 + parms[48]*x111 + x110*x160
    x175 = -x96
    x176 = x120*x165
    x177 = parms[42]*x91 + parms[44]*x98 + parms[45]*x99 + parms[47]*x136 + parms[49]*x146 + x111*x173 + x114*x116 + x115*x174 + x117*x175 + x120*x153 + x128*x159 + x154*x163 - 0.202781654946951*x176
    x178 = -x82/2
    x179 = parms[47]*x99 + parms[50]*x147 + x114*x175 + x115*x173 + x128*x165 + x160*x91 + x164
    x180 = 0.0427816549469513*x179
    x181 = parms[48]*x134 + parms[49]*x98 + parms[50]*x146 + x112*x114 + x118*x163 + x137*x161 + x176
    x182 = x181*x82
    x183 = -x124
    x184 = -parms[53]*x126 - parms[55]*x133 - parms[56]*x135 - parms[57]*x148 - x131*x152 + x139*x156 - x140*x150 - x141*x155 - x157*x183
    x185 = parms[43]*x91 + parms[45]*x98 + parms[46]*x99 + parms[47]*x147 - parms[48]*x146 + x106*x161 - x110*x173 + x113*x174 + x162*x96 + x184
    x186 = x51*x82
    x187 = -parms[47]*x98 + parms[48]*x91 + parms[50]*x107 - parms[58]*x126 - parms[60]*x136 + x113*x173 - x131*x150 - x133*x144 - x156*x183 + x161*x96
    x188 = x105*x187
    x189 = parms[31]*x56 + parms[32]*x63 + parms[33]*x64 + parms[38]*x72 + x100*x171 + x103*x169 + x166*x80 + x168*x77 - x172*x74 + x177*x178 + x180*x80 + 0.0855633098939025*x182 + x185*x186 + x188*x80 + x73*x79
    x190 = -parms[27]
    x191 = parms[28]*x32 + parms[30]*x66 + x190*x38
    x192 = -parms[29]
    x193 = parms[22]*x32 + parms[24]*x38 + parms[25]*x24 + parms[29]*x36 + x190*x66
    x194 = -x24
    x195 = parms[27]*x24 + parms[30]*x14 + x192*x32
    x196 = parms[39]*x73 + parms[40]*x71 + x74*x78
    x197 = parms[31]*x62 + parms[32]*x73 + parms[33]*x74 + parms[38]*x100 + x169*x77
    x198 = x80/2
    x199 = -x62
    x200 = x181*x80
    x201 = x54*x80
    x202 = parms[32]*x56 + parms[34]*x63 + parms[35]*x64 + parms[39]*x101 + x100*x196 + x166*x82 + x167*x72 - x168*x71 + x177*x198 + x180*x82 + x185*x201 + x188*x82 + x197*x74 + x199*x79 - 0.0855633098939025*x200
    x203 = -x43/2
    x204 = parms[37]*x64 + parms[40]*x103 + x168*x199 + x169*x56 + x179*x198 + x182 + x187*x201 + x196*x74
    x205 = 0.125040827473476*x204
    x206 = parms[39]*x63 + parms[40]*x101 + x168*x73 + x171*x74 + x178*x179 + x186*x187 + x200 + x64*x78
    x207 = x206*x43
    x208 = x177*x51 + x185/2
    x209 = -x73
    x210 = parms[33]*x56 + parms[35]*x63 + parms[36]*x64 + parms[37]*x103 + x101*x78 + x170*x71 + x172*x62 - x196*x77 + x197*x209 + x208
    x211 = x43*x51
    x212 = parms[38]*x56 + parms[40]*x72 + x167*x63 + x170*x62 + x179*x51 + x187/2 + x196*x209
    x213 = x212*x70
    x214 = parms[21]*x26 + parms[22]*x27 + parms[23]*x33 + parms[28]*x37 + x13*x192 + x14*x191 + x189*x41 + x193*x194 - x195*x66 + x202*x203 + x205*x41 + 0.250081654946951*x207 + x210*x211 + x213*x41 + x38*x40
    x215 = x202*x51 + x210/2
    x216 = parms[21]*x32 + parms[22]*x38 + parms[23]*x24 + parms[28]*x66 + x14*x192
    x217 = -x38
    x218 = parms[29]*x38 + parms[30]*x36 + x24*x39
    x219 = -x14
    x220 = parms[23]*x26 + parms[25]*x27 + parms[26]*x33 + parms[27]*x13 + x193*x32 + x195*x36 + x215 + x216*x217 + x218*x219 + x39*x67
    x221 = -parms[19]
    x222 = parms[12]*x7 + parms[14]*x4 + parms[15]*x1 + parms[17]*x219
    x223 = parms[17]*x1 + parms[20]*x12 + x221*x7
    x224 = x206*x41
    x225 = parms[29]*x27 + parms[30]*x67 + x191*x38 + x194*x195 + x203*x204 + x211*x212 + x224 + x33*x39
    x226 = x20*x225
    x227 = parms[28]*x26 + parms[30]*x37 + x190*x27 + x195*x32 + x204*x51 + x212/2 + x217*x218
    x228 = parms[11]*x7 + parms[12]*x4 + parms[13]*x1 + parms[18]*x14 + x12*x221
    x229 = parms[18]*x8 + parms[19]*x4
    x230 = -x7
    x231 = x41/2
    x232 = -x32
    x233 = x41*x54
    x234 = -x229
    x235 = parms[22]*x26 + parms[24]*x27 + parms[25]*x33 + parms[29]*x67 + x189*x43 + x190*x37 - x191*x36 + x202*x231 + x205*x43 + x210*x233 + x213*x43 + x216*x24 + x218*x66 - 0.250081654946951*x224 + x232*x40
#
    tau_out[1] = ddq[1]*parms[4] + x0*(parms[11]*x5 + parms[12]*x9 + parms[13]*x10 + parms[18]*x13 + x12*x16 + x17*x4 + x18*x214 + x20*x220 + x219*x223 + x221*x35 + x222*x8 + 0.0098*x226 + x227*x65) + x11*x3*(parms[18]*x5 + parms[20]*x13 + parms[27]*x33 + parms[30]*x13 + x15*x9 + x191*x232 + x192*x26 + x204*x231 + x207 + x212*x233 + x218*x24 + x223*x7 + x234*x4) + x3*(parms[12]*x5 + parms[14]*x9 + parms[15]*x10 + parms[19]*x34 + x1*x228 + x13*x15 + x14*x229 + x17*x230 + x20*x214 - 0.0098*x20*x227 + x220*x29 + x225*x65)
    tau_out[2] = -parms[13]*x5 - parms[15]*x9 - parms[16]*x10 - 0.41*parms[17]*x10 - parms[17]*x35 + parms[18]*x34 - 0.41*parms[20]*x35 - 0.41*x1*x229 - x12*x234 - 0.41*x16*x230 - 0.41*x221*x5 - x222*x7 - 0.41*x226 - 0.41*x227*x29 + x228*x4 - x235
    tau_out[3] = x235
    tau_out[4] = x215
    tau_out[5] = x208
    tau_out[6] = x184
#
    return tau_out
end